/*
 * Copyright (c) 2013 Johann Hanne
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
 * THE SOFTWARE.
 */

#include <stdlib.h>
#include <stdint.h>
#include <stdio.h>
#include <string.h>
#include <unistd.h>
#include <errno.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <netinet/ip.h>
#include <net/if.h>
#include <netdb.h>
#include <netinet/in.h>
#include <arpa/inet.h>

const char askew610bootmsg[] = "ASKEW610_BOOT";
const char askew610hellomsg[] = "ASKEW610_HELLO";

static int getipaddrstr(struct in_addr in_addr, char *str, size_t strsize) {
    struct sockaddr_in sockaddr_in;

    sockaddr_in.sin_family = AF_INET;
    sockaddr_in.sin_port = 0;
    sockaddr_in.sin_addr = in_addr;

    return getnameinfo((struct sockaddr *)&sockaddr_in, sizeof(sockaddr_in),
                       str, strsize, 0, 0, NI_NUMERICHOST);
}

static int boothook(struct in_addr ipaddr) {
    int retval = 0;
    int sock;
    struct sockaddr_in sockaddr;
    struct sockaddr_in sockaddr_dest;
    ssize_t recvd;
    ssize_t sent;
    uint8_t buf[32];
    int so_broadcast = 1;
    int ip_pktinfo = 1;
    struct iovec iovec;
    struct msghdr msghdr;
    uint8_t cmsgbuf[CMSG_SPACE(sizeof(struct in_pktinfo))];
    struct cmsghdr *cmsg;
    struct in_pktinfo *in_pktinfo;
    char local_addr[INET_ADDRSTRLEN];
    char header_dest_addr[INET_ADDRSTRLEN];
    unsigned int ifindex;
    uint32_t ipaddrnum;
    uint8_t mac[6];

    sock = socket(AF_INET, SOCK_DGRAM, 0);
    if (sock < 0) {
        fprintf(stderr, "socket() failed\n");
        retval = -1;
        goto cleanup;
    }

    if (setsockopt(sock, SOL_SOCKET, SO_BROADCAST, &so_broadcast, sizeof(so_broadcast)) != 0) {
        fprintf(stderr, "setsockopt(SO_BROADCAST) failed\n");
        retval = -1;
        goto cleanup;
    }

    if (setsockopt(sock, IPPROTO_IP, IP_PKTINFO, &ip_pktinfo, sizeof(ip_pktinfo)) != 0) {
        fprintf(stderr, "setsockopt(IP_PKTINFO) failed\n");
        retval = -1;
        goto cleanup;
    }

    memset(&sockaddr, 0, sizeof(sockaddr));
    sockaddr.sin_family = AF_INET;
    sockaddr.sin_port = htons(1610);
    sockaddr.sin_addr.s_addr = INADDR_ANY;

    if (bind(sock, (struct sockaddr *)&sockaddr, sizeof(sockaddr)) != 0) {
        fprintf(stderr, "bind() failed\n");
        retval = -1;
        goto cleanup;
    }

    memset(&msghdr, 0, sizeof(msghdr));
    msghdr.msg_iov = &iovec;
    msghdr.msg_iovlen = 1;

    for (;;) {

        msghdr.msg_control = cmsgbuf;
        msghdr.msg_controllen = sizeof(cmsgbuf);

        iovec.iov_base = buf;
        iovec.iov_len = sizeof(buf);

        recvd = recvmsg(sock, &msghdr, 0);
        if (recvd < 0) {
            fprintf(stderr, "recvmsg() failed\n");
            retval = -1;
            break;
        }

        if (recvd >= (strlen(askew610bootmsg) + 6) &&
            memcmp(buf, askew610bootmsg, strlen(askew610bootmsg)) == 0) {

            memcpy(mac, buf + strlen(askew610bootmsg), 6);

            printf("Received a %zd byte ASKEW610_BOOT message indicating MAC address %02x:%02x:%02x:%02x:%02x:%02x\n",
                   recvd,
                   (unsigned int)mac[0],
                   (unsigned int)mac[1],
                   (unsigned int)mac[2],
                   (unsigned int)mac[3],
                   (unsigned int)mac[4],
                   (unsigned int)mac[5]);

            cmsg = CMSG_FIRSTHDR(&msghdr);

            in_pktinfo = NULL;

            for (cmsg = CMSG_FIRSTHDR(&msghdr); cmsg != NULL; cmsg = CMSG_NXTHDR(&msghdr, cmsg)) {
                if (cmsg->cmsg_level == IPPROTO_IP && cmsg->cmsg_type == IP_PKTINFO) {
                    in_pktinfo = (struct in_pktinfo *)CMSG_DATA(cmsg);
                    break;
                }
            }

            memcpy(iovec.iov_base, askew610hellomsg, strlen(askew610hellomsg));
            ipaddrnum = ntohl(ipaddr.s_addr);
            memcpy((uint8_t *)iovec.iov_base + strlen(askew610hellomsg), mac, 6);
            ((uint8_t *)iovec.iov_base)[strlen(askew610hellomsg) + 6] = (ipaddrnum & 0xff000000) >> 24;
            ((uint8_t *)iovec.iov_base)[strlen(askew610hellomsg) + 7] = (ipaddrnum & 0x00ff0000) >> 16;
            ((uint8_t *)iovec.iov_base)[strlen(askew610hellomsg) + 8] = (ipaddrnum & 0x0000ff00) >>  8;
            ((uint8_t *)iovec.iov_base)[strlen(askew610hellomsg) + 9] =  ipaddrnum & 0x000000ff;
            iovec.iov_len = strlen(askew610hellomsg) + 6 + 4;

            memset(&sockaddr_dest, 0, sizeof(sockaddr_dest));
            sockaddr_dest.sin_family = AF_INET;
            sockaddr_dest.sin_port = htons(1610);
            sockaddr_dest.sin_addr.s_addr = INADDR_BROADCAST;

            if (in_pktinfo == NULL) {
                fprintf(stderr, "Warning: Failed to get IP packet info - reply might be sent on wrong network interface\n");

                sent = sendto(sock, iovec.iov_base, iovec.iov_len, 0, (struct sockaddr *)&sockaddr_dest, sizeof(sockaddr_dest));
                if (sent < 0) {
                    fprintf(stderr, "sendto() failed\n");
                    retval = -1;
                    break;
                } else if (sent < iovec.iov_len) {
                    fprintf(stderr, "Warning: Did not send all bytes of response\n");
                }
            } else {
                getipaddrstr(in_pktinfo->ipi_spec_dst, local_addr, sizeof(local_addr));
                getipaddrstr(in_pktinfo->ipi_addr, header_dest_addr, sizeof(header_dest_addr));
                ifindex = in_pktinfo->ipi_ifindex;

                printf("- interface index: %u\n- destination address: %s\n- local interface address: %s\n",
                       ifindex, header_dest_addr, local_addr);

                cmsg = CMSG_FIRSTHDR(&msghdr);
                cmsg->cmsg_level = IPPROTO_IP;
                cmsg->cmsg_type = IP_PKTINFO;
                cmsg->cmsg_len = CMSG_LEN(sizeof(*in_pktinfo));
                in_pktinfo = (struct in_pktinfo *)CMSG_DATA(cmsg);
                memset(in_pktinfo, 0, sizeof(*in_pktinfo));
                in_pktinfo->ipi_ifindex = ifindex;
                msghdr.msg_control = cmsgbuf;
                msghdr.msg_controllen = cmsg->cmsg_len;
                msghdr.msg_flags = 0;

                msghdr.msg_name = &sockaddr_dest;
                msghdr.msg_namelen = sizeof(sockaddr_dest);

                sent = sendmsg(sock, &msghdr, 0);
                if (sent < 0) {
                    fprintf(stderr, "sendmsg() failed\n");
                    retval = -1;
                    break;
                } else if (sent < iovec.iov_len) {
                    fprintf(stderr, "Warning: Did not send all bytes of response\n");
                }

                printf("Sent %zd bytes\n", sent);
            }
        }
    }

cleanup:

    if (sock >= 0) {
        close(sock);
    }

    return retval;
}

int main(int argc, char *argv[]) {
    struct in_addr ipaddr;

    if (argc != 2) {
        fprintf(stderr, "Syntax: %s <ip-address>\n", argv[0]);
        return EXIT_FAILURE;
    }

    if (inet_aton(argv[1], &ipaddr) == 0) {
        fprintf(stderr, "Invalid IP address: '%s'\n", argv[1]);
        return EXIT_FAILURE;
    }

    return (boothook(ipaddr) == 0) ? EXIT_SUCCESS : EXIT_FAILURE;
}
